#include "innfos_kinematics.h"
#include "AR10_kinematics.h"
#include "remote_control.h"
#include <unistd.h>
#include <iostream>
#include "plot_realtime.h"
#include <Eigen/Eigen>

using namespace std;
using namespace Eigen;

int main()
{
	//plot_realtime plt;
	/**double arm; one follows another**/
	/*
	remote_control rc("enp2s0", 91, 91);
	rc.on(false);
	
	remote_control rc1("enp2s0", 131, 131);
	rc1.on(false);
	
	this_thread::sleep_for(std::chrono::seconds(1));
	
	double joint_initail[6] = {0, 0, 0, 0, 0, 0};
	
	double sensi[6] = {100, 0.5, 0.25, 0.05, 0.5, 100};
	double speed[6] = {1, 800, 1200, 800, 1, 1};
	
	rc.adject_dragging_speed(speed);
	rc.adject_sensitivity(sensi);
	rc1.adject_dragging_speed(speed);
	rc1.adject_sensitivity(sensi);
	
	rc.follow(joint_initail);
	rc1.follow(joint_initail);
	this_thread::sleep_for(std::chrono::seconds(10));
	rc1.follow_stop();
	
	rc1.dragging(joint_initail);
	for (int i = 0; i<2000; i++)
	{
		cout << "joint_initail: " << joint_initail[0] << "; " << joint_initail[1] << "; " << joint_initail[2] << "; " << joint_initail[3] << "; " << joint_initail[4] << "; " << joint_initail[5] << endl;
		this_thread::sleep_for(std::chrono::milliseconds(10));
	}
	rc1.dragging_stop();
	rc.follow_stop();
	
	rc.off();
	rc1.off();
	*/
	
	
	/**adjust sensitive of remote controller**/
	/*
	remote_control rc("enp2s0", 91, 131);
	rc.on(false);
	
	this_thread::sleep_for(std::chrono::seconds(1));
	
	double joint_initail[6] = {0, 0, 0, 0, 0, 0};
	double sensi[6] = {100, 0.5, 0.25, 0.05, 0.5, 100};
	double speed[6] = {1, 800, 1200, 800, 1, 1};
	
	rc.adject_dragging_speed(speed);
	rc.adject_sensitivity(sensi);
	
	rc.follow(joint_initail);
	this_thread::sleep_for(std::chrono::seconds(3));
	rc.follow_stop();
	
	plt.plot(joint_initail[2]);
	this_thread::sleep_for(std::chrono::seconds(1));
	
	rc.dragging(joint_initail);
	for (int i = 0; i<5000; i++)
	{
		//cout << "joint_initail: " << joint_initail[0] << "; " << joint_initail[1] << "; " << joint_initail[2] << "; " << joint_initail[3] << "; " << joint_initail[4] << "; " << joint_initail[5] << endl;
		this_thread::sleep_for(std::chrono::milliseconds(10));
	}
	rc.dragging_stop();
	
	rc.off();
	*/
	
	/**double arm; one follows another by end point**/
	/*
	remote_control rc("enp2s0", 91, 91);
	rc.on(false);
	
	remote_control rc1("enp2s0", 131, 131);
	rc1.on(false);
	
	innfos_kinematics innfos_kine;
	
	this_thread::sleep_for(std::chrono::seconds(1));
	
	Vector6 joints;
	joints << 0, M_PI/4, M_PI/2, 0, M_PI/2, 0;
	
	Matrix4d m_4d;
	m_4d << 0.485,   -0.87,    0.04,    -0.445,
    -0.874,  -0.483,   0.05,   -0.793,
    -0.023,    -0.062,   -0.998,    -0.431,
         0,         0,         0,         1;
    Isometry3d transform;
    for (int i = 0; i < 4; i++)
    {
    	for (int j=0; j<4; j++)
    	{
    		transform(i,j) = m_4d(i,j);
    	}
    }
	cout << transform.matrix() << endl;
	
	double sensi[6] = {100, 0.5, 0.25, 0.05, 0.5, 100};
	double speed[6] = {1, 800, 1200, 800, 1, 1};
	
	rc.adject_dragging_speed(speed);
	rc.adject_sensitivity(sensi);
	rc1.adject_dragging_speed(speed);
	rc1.adject_sensitivity(sensi);
	
	rc.follow(transform, M_PI_2, -M_PI_2, -M_PI_4, 1, 3, 0.0, 0.3, 0.3);
	rc1.follow(transform, M_PI_2, -M_PI_2, -M_PI_4, 1, 3, 0.0, 0.3, 0.3);
	this_thread::sleep_for(std::chrono::seconds(1));
	rc1.follow_stop();
	
	rc1.dragging(transform, M_PI_2, -M_PI_2, -M_PI_4, 1, 3, 0.0, 0.3, 0.3);
	for (int i = 0; i<10; i++)
	{
		cout << "transform: " << transform.matrix() << endl;
		this_thread::sleep_for(std::chrono::milliseconds(10));
	}
	rc1.dragging_stop();
	rc.follow_stop();
	
	rc.off();
	rc1.off();
	*/
	
	/**one arm test **/
	/*
	remote_control rc1("enp2s0", 131, 131);
	rc1.on(false);
	
	innfos_kinematics innfos_kine;
	
	this_thread::sleep_for(std::chrono::seconds(1));
	
	Vector6 joints;
	joints << 0, M_PI/4, M_PI/2, 0, M_PI/2, 0;
	
	Isometry3d transform = innfos_kine.baseTend(joints);
	//cout << transform.matrix() << endl;
	
	double sensi[6] = {100, 0.5, 0.25, 0.05, 0.5, 100};
	double speed[6] = {1, 800, 1200, 800, 1, 1};
	
	rc1.adject_dragging_speed(speed);
	rc1.adject_sensitivity(sensi);
	
	rc1.follow(transform);
	this_thread::sleep_for(std::chrono::seconds(3));
	rc1.follow_stop();
	
	rc1.dragging(transform, 0, 0, 0, 1, 0.1, 0 ,0);
	for (int i = 0; i<300; i++)
	{
		//cout << "transform: " << transform.matrix() << endl;
		this_thread::sleep_for(std::chrono::milliseconds(10));
	}
	rc1.dragging_stop();
	
	rc1.off();
	*/
	
	/**one arm endpoint follow test to control ar10**/
	/*
	remote_control rc1("enp2s0", 171, 171);
	rc1.on(false);
	innfos_kinematics innfos_kine;
	AR10_kinematics ar10_kine;
	Vector6 q, target_q;
	q << 0, 0, 0, 0, 0, 0;
	
	this_thread::sleep_for(std::chrono::seconds(1));
	
	Isometry3d transform = Isometry3d::Identity();
	rc1.dragging(transform, 0.0, 0.3, 0.4, M_PI_2, -M_PI_2, -M_PI_4, 1, 3);
	
	ar10_kine.getQ(transform, q, target_q);
	q = target_q;
	cout << target_q << endl; 
	vector<int> ratio = {1, 1, 1, 1, 1, 0};
	double threahold = M_PI/18;
	for (int i = 0; i<3000; i++)
	{
		//cout << "transform: " << transform.matrix() << endl;
		if (ar10_kine.getQ(transform, q, target_q, ratio, threahold) != -2)
		{
			q = target_q;
			cout << target_q << endl; 
		}
		else
		{
			cout << "q: " << q << endl;
			cout << "target_q: " << target_q << endl;
			break;
		}
		this_thread::sleep_for(std::chrono::milliseconds(2));
	}
	rc1.dragging_stop();
	
	rc1.off();
	
	this_thread::sleep_for(std::chrono::milliseconds(100));
	*/
	
	//plt.exit();
	remote_control rc1("enp88s0", 91, 91);
	rc1.on(true);
	
	double joint_position[6] = {0, 0, 0, 0, 0, 0};
	rc1.dragging(joint_position);
	for (int i = 0; i<1000; i++)
	{
		//cout << "transform: " << transform.matrix() << endl;
		this_thread::sleep_for(std::chrono::milliseconds(10));
	}
	rc1.dragging_stop();
	
	rc1.off();
		
	
	return 0;
}
